#include "stdafx.h"
#include <algorithm>
#include <functional>
#include <queue>
#include "cvxSf.h"
#include "cvxImage.h"

using std::cerr;
using std::cout;
using std::endl;
using std::transform;
using std::divides;
using std::minus;
using std::queue;
using std::priority_queue;
using std::make_pair;
using std::pair;
using std::copy;
using std::ostream_iterator;

using cvx::RgbImage;
using cvx::BwImageShortInt;
using cvx::BwImage;
using cvx::BwImageInt;
using cvx::BwImageFloat;
using cvx::HsvImage;
using cvx::RgbImageFloat;
using cvx::MatFloat;
using cvx::RgbaImageFloat;

void CvxSF::saveImageVec(vector<IplImage*>& image, string prefix, string suffix, const char* dirctory)
{
	if(image.empty())
	{
		return;
	}

	vector<IplImage*>::iterator iI = image.begin();	
	for (; iI != image.end(); ++iI)
	{
		IplImage* saveImg = *iI;
		char temp[64];
		string str;
		sprintf(temp, "%d", iI - image.begin());
		string sName(temp);
		sName = string(dirctory) + prefix + sName + suffix;
		cvSaveImage(sName.c_str(), saveImg);
	}
}

void CvxSF::releaseImage(vector<IplImage*>& inoutImg)
{
	for (vector<IplImage*>::iterator iI = inoutImg.begin();
		iI != inoutImg.end(); ++iI)
	{
		if (*iI){
			cvReleaseImage(&*iI);
			*iI = NULL;
		}
	}
	inoutImg.clear();
}

bool CvxSF::GetChessboard(IplImage *inImage, bool accurate, const CvSize &size,vector<Point2f> &pts)
{
	assert(inImage);
	pts.clear();

	IplImage *grayImage = NULL;
	int w = inImage->width;
	int h = inImage->height;

	if (inImage->nChannels == 3)
	{
		grayImage = cvCreateImage(cvSize(w, h), IPL_DEPTH_8U, 1);		
		cvCvtColor(inImage, grayImage, CV_BGR2GRAY);
	}
	else
	{
		grayImage = inImage;
	}

	pts.resize(size.width * size.height);
	bool found = cv::findChessboardCorners(Mat(grayImage), size, pts, 
										   CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);
	if (!found)
	{
		if (grayImage != inImage)
		{
			pts.resize(0);
			cvReleaseImage(&grayImage);			
		}
		return false;
	}
	int winLength = 11;
	if (grayImage->width < 800)
	{
		winLength = 5;
	}

	if (accurate)
	{
		
		cv::find4QuadCornerSubpix(Mat(grayImage), pts, cvSize(winLength, winLength));		
	}
	else
	{
	//	double tt = clock();
		cv::cornerSubPix(Mat(grayImage), pts, Size(winLength, winLength), Size(-1, -1), 
						 TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
	//	printf("subpix cost time = %f\n", clock() - tt);
	}
	if (grayImage != inImage)
	{
		cvReleaseImage(&grayImage);
	}
	if (pts[0].y > pts[size.width].y)
	{
		std::reverse(pts.begin(), pts.end());
	}
	return true;	
}

bool CvxSF::GetInsidePoints(IplImage *grayImage, vector<Point2f> &extremeCorners, 
									  const CvSize &patternSize, vector<Point2f> &corners,
									  bool subpixel)
{
	assert(grayImage && grayImage->nChannels == 1);
	assert(patternSize.width > 2 && patternSize.height > 2);
	assert(extremeCorners.size() == 4);

	//find sub pix of extreme corners, and homography from object plane to image plane
	corners.resize(patternSize.width * patternSize.height);
	cv::cornerSubPix(Mat(grayImage), extremeCorners, Size(5, 5), Size(-1,-1), 
		TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
	vector<Point2f> patternVec(4);
	patternVec[0] = Point2f(0, 0);
	patternVec[1] = Point2f(patternSize.width-1, 0);
	patternVec[2] = Point2f(patternSize.width-1, patternSize.height-1);
	patternVec[3] = Point2f(0, patternSize.height-1);	
	Mat H = cv::findHomographyNorm(patternVec, extremeCorners, 0, 0);  // getPerspectiveTransform  getRectSubPix

	//Compute the inside points through computation of the planar homography (collineation)
	double *pData = (double*)H.data;
	for (int y = 0; y<patternSize.height; ++y)
	{
		for (int x = 0; x<patternSize.width; ++x)
		{
			Point2f p1(x, y);
			Point2f p2;
			double scale = 1.0;
			p2.x  = pData[0] * p1.x  + pData[1] * p1.y + pData[2];
			p2.y  = pData[3] * p1.x  + pData[4] * p1.y + pData[5];
			scale = pData[6] * p1.x  + pData[7] * p1.y + pData[8];
			p2.x /= scale;
			p2.y /= scale;
			corners[y * patternSize.width + x] = p2;
		}
	}
	if (subpixel)
	{
		int winLength = 11;
		if (grayImage->width < 800)
		{
			winLength = 5;
		}
		cv::cornerSubPix(Mat(grayImage), corners, Size(winLength, winLength), Size(-1,-1), 
						TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
	}	
	return true;
}

void CvxSF::findSaveChessboard(const vector<IplImage*> imageVec, FILE *pf, const CvSize &size, bool checkUpdown)
{
	assert(pf);
	for (int i = 0; i<imageVec.size(); ++i)
	{
		vector<Point2f> pts;
		bool isOK =  CvxSF::GetChessboard(imageVec[i], true, size, pts);
		fprintf(pf, "%d\n", isOK);
		if (isOK)
		{
			if (checkUpdown && pts[0].y > pts[size.width].y)
			{
				std::reverse(pts.begin(), pts.end());
			}
			
			for (int i = 0; i<pts.size(); ++i)
			{
				Point2f p = pts[i];
				fprintf(pf, "%f %f ", p.x, p.y);
			}
			fprintf(pf, "\n");
		}	
	}
}

void CvxSF::loadChessboard(FILE *pf, vector<int> &idx, vector<vector<Point2f> > &chessboardPos, CvSize &size)
{
	assert(pf);
	idx.clear();
	chessboardPos.clear();

	int num = 0;
	fscanf(pf, "%d", &num);
	fscanf(pf, "%d %d", &size.width, &size.height);
	for (int i = 0; i<num; ++i)
	{
		int isOk = 0;
		fscanf(pf, "%d", &isOk);
		if (isOk)
		{
			vector<Point2f> pts(size.width*size.height);
			idx.push_back(i);
			for (int j = 0; j<size.width*size.height; ++j)
			{
				float x, y;
				fscanf(pf, "%f %f", &x, &y);
				pts[j] = Point2f(x, y);
			}
			chessboardPos.push_back(pts);
		}
	}

}

void CvxSF::findSaveChessboard(const string &videoFileName, const string &savefileName, const CvSize& size, bool checkUpdown)
{
	assert(!videoFileName.empty());
	CvCapture *pCp = cvCaptureFromFile(videoFileName.c_str());
	assert(pCp);
	int frameNum = (int)cvGetCaptureProperty(pCp, CV_CAP_PROP_FRAME_COUNT);	
	FILE *fp = fopen(savefileName.c_str(), "w");
	assert(fp);
	fprintf(fp, "%s #video_file_name\n", videoFileName.c_str());
	fprintf(fp, "%d %d %d #chessboard_size_frame_number\n", size.width, size.height, frameNum);

	IplImage *frame = cvQueryFrame(pCp);	
	while(frame)
	{
		vector<Point2f> pts;
		bool isOK =  CvxSF::GetChessboard(frame, true, size, pts);
		fprintf(fp, "%d\n", isOK);
		if (isOK)
		{
			if (checkUpdown && pts[0].y > pts[size.width].y)
			{
				std::reverse(pts.begin(), pts.end());
			}

			for (int i = 0; i<pts.size(); ++i)
			{
				Point2f p = pts[i];
				fprintf(fp, "%f %f ", p.x, p.y);
			}
			fprintf(fp, "\n");
		}
		frame = cvQueryFrame(pCp);
	}
	fclose(fp);
	cvReleaseCapture(&pCp);
}

void CvxSF::saveChessboard(const string &videoFileName, const string &savefileName, 
						   const vector<vector<Point2f>>&pts, const CvSize &size)
{
	assert(!pts.empty());

	int frameNum = pts.size();
	FILE *fp = fopen(savefileName.c_str(), "w");
	assert(fp);
	fprintf(fp, "%s #video_file_name\n", videoFileName.c_str());
	fprintf(fp, "%d %d %d #chessboard_size_frame_number\n", size.width, size.height, frameNum);

	for (int i = 0; i<pts.size(); ++i)
	{
		bool isOK = (pts[i].size() == size.width * size.height);
		fprintf(fp, "%d\n", isOK);
		if (isOK)
		{			
			for (int j = 0; j<pts[i].size(); ++j)
			{
				Point2f p = pts[i][j];
				fprintf(fp, "%f %f ", p.x, p.y);
			}
			fprintf(fp, "\n");
		}
	}
	fclose(fp);
}

void CvxSF::saveStereoChessboard(const string &videoLeft, const string &videoRight, const string &saveFileName, 
								 int step, const CvSize &size, bool checkUpdown)
{
	assert(!videoLeft.empty());
	assert(!videoRight.empty());

	CvCapture *pLeftCp = cvCaptureFromFile(videoLeft.c_str());
	assert(pLeftCp);
	CvCapture *pRightCp = cvCaptureFromFile(videoRight.c_str());
	assert(pRightCp);

	int leftFrameNum = (int)cvGetCaptureProperty(pLeftCp, CV_CAP_PROP_FRAME_COUNT);	
	int rightFrameNum = (int)cvGetCaptureProperty(pRightCp, CV_CAP_PROP_FRAME_COUNT);
	int frameNum = std::min(leftFrameNum, rightFrameNum);

	FILE *fp = fopen(saveFileName.c_str(), "w");
	assert(fp);
	fprintf(fp, "%s %s #video_file_name\n", videoLeft.c_str(), videoRight.c_str());
	fprintf(fp, "%d %d %d #chessboard_size_frame_number\n", size.width, size.height, frameNum/step);

	IplImage *leftFrame  = cvQueryFrame(pLeftCp);	
	IplImage *rightFrame = cvQueryFrame(pRightCp);
	int frame = 0;
	while(leftFrame && rightFrame)
	{
		if ((frame++)%step == 0)
		{
			vector<Point2f> leftPts;
			vector<Point2f> rightPts;
			bool isOK =  CvxSF::GetChessboard(leftFrame, true, size, leftPts);
			if (isOK)
			{
				isOK = CvxSF::GetChessboard(rightFrame, true, size, rightPts);			
			}
			fprintf(fp, "%d\n", isOK);
			if (isOK)
			{
				if (checkUpdown)
				{
					if (leftPts[0].y > leftPts[size.width].y)
					{
						std::reverse(leftPts.begin(), leftPts.end());
					}
					if (rightPts[0].y > rightPts[size.width].y)
					{
						std::reverse(rightPts.begin(), rightPts.end());
					}				
				}
				for (int i = 0; i<leftPts.size(); ++i)
				{
					Point2f p = leftPts[i];
					fprintf(fp, "%f %f ", p.x, p.y);
				}
				fprintf(fp, "\n");

				for (int i = 0; i<rightPts.size(); ++i)
				{
					Point2f p = rightPts[i];
					fprintf(fp, "%f %f ", p.x, p.y);
				}
				fprintf(fp, "\n");

			}			
		}	
		leftFrame  = cvQueryFrame(pLeftCp);
		rightFrame = cvQueryFrame(pRightCp);
	}
	fclose(fp);
	cvReleaseCapture(&pLeftCp);
	cvReleaseCapture(&pRightCp);
}

void CvxSF::loadStereoChessboard(const string &fileName, string &leftVideo, string &rightVideo, 
						  vector<int> &idx, CvSize &size, vector<vector<Point2f>> &leftPts, vector<vector<Point2f>> &rightPts)
{
	FILE *pf = fopen(fileName.c_str(), "r");
	assert(pf);
	char line[255] = {0};
	int frameN = 0;
	fgets(line, 255, pf);
	//fprintf(stdout, "%s\n", line);  //video file name
	sscanf(line, "%s %s", leftVideo.c_str(), rightVideo.c_str());
//	fprintf(stdout, "%s %s\n", leftVideo.c_str(), rightVideo.c_str());
	memset(line, 0, sizeof(line));
	fgets(line, 255, pf);          //#chessboard size & frame number
	sscanf(line, "%d %d %d", &size.width, &size.height, &frameN);
	//fprintf(stdout, "%d %d %d\n", size.width, size.height, frameN); 

	for (int i = 0; i<frameN; ++i)
	{
		int isOk = 0;
		fscanf(pf, "%d", &isOk);
		if (isOk)
		{
			vector<Point2f> pts;
			float x, y;
			for (int j = 0; j<size.width * size.height; ++j)
			{
				fscanf(pf, "%f %f ", &x, &y);				
				pts.push_back(Point2f(x, y));
			}
			leftPts.push_back(pts);

			pts.clear();
			for (int j = 0; j<size.width * size.height; ++j	)
			{
				fscanf(pf, "%f %f ", &x, &y);
				pts.push_back(Point2f(x, y));
			}
			rightPts.push_back(pts);
			idx.push_back(i);
		}		
	}
	fclose(pf);
}

void CvxSF::loadChessboard(const string &fileName, string &videoFileName, vector<int> &idx, vector<vector<Point2f> > &chessboardPos, CvSize &size)
{
	FILE *pf = fopen(fileName.c_str(), "r");
	assert(pf);
	char line[255] = {0};
	char vFileName[255] = {0};
	int frameN = 0;
	fgets(line, 255, pf);
	//fprintf(stdout, "%s\n", line);  //video file name
	sscanf(line, "%s ", vFileName);
	videoFileName = string(vFileName);
	//fprintf(stdout, "%s\n", videoFileName);
	memset(line, 0, sizeof(line));
	fgets(line, 255, pf);          //#chessboard size & frame number
	sscanf(line, "%d %d %d", &size.width, &size.height, &frameN);
	//fprintf(stdout, "%d %d %d\n", size.width, size.height, frameN); 

	for (int i = 0; i<frameN; ++i)
	{
		int isOk = 0;
		fscanf(pf, "%d", &isOk);
		if (isOk)
		{
			idx.push_back(i);
			vector<Point2f> pts;
			for (int j = 0; j<size.width * size.height; ++j)
			{
				float x, y;
				fscanf(pf, "%f %f ", &x, &y);
				Point2f p(x, y);
				pts.push_back(p);
			}
			chessboardPos.push_back(pts);
		}
	}
	fclose(pf);
}

bool CvxSF::LeastSquare(const vector<map<int, double> > &matrix_vec, 
				 const vector<double> & right_vec, vector<double> &result)
{
	assert(matrix_vec.size() == right_vec.size());
	assert(result.size() != 0);

	Mat A = Mat::zeros(matrix_vec.size(), result.size(), CV_64F);
	Mat b = Mat::zeros(matrix_vec.size(), 1, CV_64F);

	for(int i = 0; i<matrix_vec.size(); ++i)
	{
		for(map<int, double>::const_iterator j = matrix_vec[i].begin(); j!=matrix_vec[i].end(); ++j)
		{
			A.at<double>(i, j->first) = j->second;
		}
	}
	for(int i = 0; i<right_vec.size(); ++i)
	{
		b.at<double>(i) = right_vec[i];
	}
	Mat x = Mat(result.size(), 1, CV_64F, &result[0]);	
	x = (A.t()*A).inv()*(A.t()*b);		
	return true;
}
